#ifndef IMU_H
#define IMU_H

#include "basicio.h"

#include <stdio.h>
#include "esp_log.h"
#include "driver/i2c.h"
#include "sdkconfig.h"

#include <string.h>
#include <math.h>

//#include "lis2dh12_reg.h"

#define I2C_MASTER_NUM              1
#define I2C_MASTER_TX_BUF_DISABLE   0                               /*!< I2C master doesn't need buffer */
#define I2C_MASTER_RX_BUF_DISABLE   0                               /*!< I2C master doesn't need buffer */
#define WRITE_BIT                   I2C_MASTER_WRITE                /*!< I2C master write */
#define READ_BIT                    I2C_MASTER_READ                 /*!< I2C master read */
#define ACK_CHECK_EN                0x1                             /*!< I2C master will check ack from slave*/
#define ACK_CHECK_DIS               0x0                             /*!< I2C master will not check ack from slave */
#define ACK_VAL                     0x0                             /*!< I2C ack value */
#define NACK_VAL                    0x1                             /*!< I2C nack value */

#define LIS2DH12_IIC_Addr           0x19                            //SDO-VDDIO:0x19(0011001b)  SDO-GND:0x18(0011000b)

#define STATUS_REG_AUX              0x07
#define WHO_AM_I                    0x0f
#define OUT_TEMP_L                  0x0c
#define OUT_TEMP_H                  0x0d
#define CTRL_REG0                   0x1e
#define TEMP_CFG_REG                0x1f
#define CTRL_REG1                   0x20
#define CTRL_REG2                   0x21
#define CTRL_REG3                   0x22
#define CTRL_REG4                   0x23
#define CTRL_REG5                   0x24
#define CTRL_REG6                   0x25
#define REFERENCE                   0x26
#define STATUS_REG                  0x27

#define OUT_X_L                     0x28
#define OUT_X_H                     0x29
#define OUT_Y_L                     0x2a
#define OUT_Y_H                     0x2b
#define OUT_Z_L                     0x2c
#define OUT_Z_H                     0x2d

#define FIFO_CTRL_REG               0x2e
#define FIFO_SRC_REG                0x2f
#define INT1_CFG                    0x30
#define INT1_SRC                    0x31
#define INT1_THS                    0x32
#define INT1_DURATION               0x33
#define INT2_CFG                    0x34
#define INT2_SRC                    0x35
#define INT2_THS                    0x36
#define INT2_DURATION               0x37

#define CLICK_CFG                   0x38
#define CLICK_SRC                   0x39
#define CLICK_THS                   0x3a
#define TIME_LIMIT                  0x3b
#define TIME_LATENCY                0x3c
#define TIME_WINDOW                 0x3d

#define ACT_THS                     0x3e
#define ACT_DUR                     0x3f

#define LIS2DH12_FROM_FS_2g_HR_TO_mg(lsb)  (float)((int16_t)lsb >> 4) * 1.0f
#define LIS2DH12_FROM_FS_4g_HR_TO_mg(lsb)  (float)((int16_t)lsb >> 4) * 2.0f
#define LIS2DH12_FROM_FS_8g_HR_TO_mg(lsb)  (float)((int16_t)lsb >> 4) * 4.0f
#define LIS2DH12_FROM_FS_16g_HR_TO_mg(lsb) (float)((int16_t)lsb >> 4) * 12.0f 
#define LIS2DH12_FROM_LSB_TO_degC_HR(lsb)  (float)((int16_t)lsb >> 6) / 4.0f + 25.0f

#define DEGREE_CAL                          180.0 / 3.1416
#define FILTER_CNT                          4

typedef struct
{
    int16_t ax;
    int16_t ay;
    int16_t az;
    uint16_t angle;

    uint8_t id;
    bool intstat1;
    bool intstat2;
}IMU_Type;
extern IMU_Type imu;

typedef struct
{
    int16_t ax;
    int16_t ay;
    int16_t az;
}SMP_Type;
extern SMP_Type smp;

void imu_init(void);
int16_t imu_write(uint8_t reg_addr, uint8_t data);
int16_t imu_read(uint8_t reg_addr, uint8_t *data_rd);
void accel_read(void);
void angle_calc(void);

#endif

